//
// Created by cch on 2022/5/1.
//

#ifndef IMU_GPS_EKF_IMU_GPS_FUSION_H
#define IMU_GPS_EKF_IMU_GPS_FUSION_H

#include "base_type.h"
#include "gps_processer.h"
#include "imu_processer.h"
#include "initalizer.h"
#include <memory>

namespace IMUGPS {
class Fusion {
public:
  Fusion(const double acc_noise,const double gyro_nosie,
         const double acc_bias_noise,const double gyro_bias_noise,
         const Eigen::Vector3d &I_P_GPS);
  bool ProcessImuData(const ImuDataPtr &imuDataPtr,State * state);
  void ProcessGpsData(const GpsDataPtr &gpsDataPtr);

private:
  std::unique_ptr<Initializer> initializer;
  std::unique_ptr<ImuProcess> imuProcess;
  std::unique_ptr<GpsProcesser> gpsProcesser;

  Eigen::Vector3d init_lla;
  State *state_;
};
} // namespace IMUGPS
#endif // IMU_GPS_EKF_IMU_GPS_FUSION_H
